import ConvexGeometry from "./convex-geometry";
import CONSTANT from '../constant';

export default class ConeGeometry extends ConvexGeometry{
    constructor(radius,height) {
        super(CONSTANT.GEOMETRY_TYPE_CONE);
        this._radius = radius;
        let hh=height*0.5;
        this._halfHeight = hh;
        this.sinTheta = radius / Math.sqrt(radius * radius + 4 * hh * hh);
        this.cosTheta = 2 * hh / Math.sqrt(radius * radius + 4 * hh * hh);
        this._updateMass();
    }
    getRadius() {
        return this._radius;
    }
    getHalfHeight() {
        return this._halfHeight;
    }
    _updateMass() {
        let r=this._radius,hh=this._halfHeight;
        let r2 = r * r;
        let h2 = hh * hh * 4;
        this._volume = 3.14159265358979 * r2 * hh * 2 / 3;
        let icf=this.inertiaCoeff;
        icf[0] = 0.05 * (3 * r2 + 2 * h2);
        icf[1] = 0;
        icf[2] = 0;
        icf[3] = 0;
        icf[4] = 0.3 * r2;
        icf[5] = 0;
        icf[6] = 0;
        icf[7] = 0;
        icf[8] = 0.05 * (3 * r2 + 2 * h2);
    }
    _computeAabb(_aabb,_tf) {
        let aabb=_aabb.elements,tf=_tf.elements,r=this._radius,hh=this._halfHeight;
        let axisX = tf[4],axisY = tf[7],axisZ = tf[10];
        let axis2X = axisX * axisX,axis2Y = axisY * axisY,axis2Z = axisZ * axisZ;
        let erX = Math.sqrt(1 - axis2X),erY = Math.sqrt(1 - axis2Y),erZ = Math.sqrt(1 - axis2Z);
        erX *= r;erY *= r;erZ *= r;
        let ehX = axisX * hh,ehY = axisY * hh,ehZ = axisZ * hh;
        let rminX = -ehX,rminY = -ehY,rminZ = -ehZ;
        rminX -= erX;rminY -= erY;rminZ -= erZ;
        let rmaxX = -ehX,rmaxY = -ehY,rmaxZ = -ehZ;
        rmaxX += erX;rmaxY += erY;rmaxZ += erZ;
        let maxX = rminX > rmaxX ? rminX : rmaxX,maxY = rminY > rmaxY ? rminY : rmaxY,maxZ = rminZ > rmaxZ ? rminZ : rmaxZ;
        if(!(maxX > ehX)) maxX = ehX;
        if(!(maxY > ehY)) maxY = ehY;
        if(!(maxZ > ehZ)) maxZ = ehZ;
        let minX = rminX < rmaxX ? rminX : rmaxX,minY = rminY < rmaxY ? rminY : rmaxY,minZ = rminZ < rmaxZ ? rminZ : rmaxZ;
        if(!(minX < ehX)) minX = ehX;
        if(!(minY < ehY)) minY = ehY;
        if(!(minZ < ehZ)) minZ = ehZ;
        aabb[0] = tf[0] + minX;
        aabb[1] = tf[1] + minY;
        aabb[2] = tf[2] + minZ;
        aabb[3] = tf[0] + maxX;
        aabb[4] = tf[1] + maxY;
        aabb[5] = tf[2] + maxZ;
    }
    computeLocalSupportingVertex(_dir,_out) {
        let dir=_dir.elements,out=_out.elements;
        let sinTheta=this.sinTheta,hh=this._halfHeight,gjm=this._gjkMargin;
        let dx = dir[0], dy = dir[1], dz = dir[2];
        if(dy > 0 && dy * dy > sinTheta * sinTheta * (dx * dx + dy * dy + dz * dz)) {
            out[1]=hh - gjm / sinTheta;
            out[0]=out[2]=0;
            if(out[1] < 0) out[1] = 0;
            return;
        }
        let rx = dir[0];
        let rz = dir[2];
        let len = rx * rx + rz * rz;
        let height = 2 * hh;
        let coreRadius = (height - gjm) / height * this._radius - gjm / this.cosTheta;
        if(coreRadius < 0) coreRadius = 0;
        let invLen = len > 0 ? coreRadius / Math.sqrt(len) : 0;
        let coreHalfHeight = hh - gjm;
        if(coreHalfHeight < 0) coreHalfHeight = 0;
        out[0] = rx * invLen;out[1] = -coreHalfHeight;out[2] = rz * invLen;
    }
    _rayCastLocal(beginX,beginY,beginZ,endX,endY,endZ,hit) {
        let p1y;
        let halfH = this._halfHeight,cosTheta=this.cosTheta;
        let dx = endX - beginX, dy = endY - beginY, dz = endZ - beginZ;
        let tminy = 0, tmaxy = 1;
        if(dy > -1e-6 && dy < 1e-6) {
            if(beginY <= -halfH || beginY >= halfH) return false;
        } else {
            let invDy = 1 / dy;
            let t1 = (-halfH - beginY) * invDy;
            let t2 = (halfH - beginY) * invDy;
            if(t1 > t2) {
                let tmp = t1;
                t1 = t2;
                t2 = tmp;
            }
            if(t1 > 0) tminy = t1;
            if(t2 < 1) tmaxy = t2;
        }
        if(tminy >= 1 || tmaxy <= 0) return false;
        let tminxz = 0, tmaxxz = 0;
        p1y = beginY - halfH;
        let cos2 = cosTheta * cosTheta;
        let a = cos2 * (dx * dx + dy * dy + dz * dz) - dy * dy;
        let b = cos2 * (beginX * dx + p1y * dy + beginZ * dz) - p1y * dy;
        let c = cos2 * (beginX * beginX + p1y * p1y + beginZ * beginZ) - p1y * p1y;
        let D = b * b - a * c;
        if(a !== 0) {
            if(D < 0) return false;
            let sqrtD = Math.sqrt(D);
            if(a < 0) {
                if(dy > 0) {
                    tminxz = 0;
                    tmaxxz = (-b + sqrtD) / a;
                    if(tmaxxz <= 0) return false;
                } else {
                    tminxz = (-b - sqrtD) / a;
                    tmaxxz = 1;
                    if(tminxz >= 1) return false;
                }
            } else {
                tminxz = (-b - sqrtD) / a;
                tmaxxz = (-b + sqrtD) / a;
                if(tminxz >= 1 || tmaxxz <= 0) return false;
            }
        } else {
            let t = -c / (2 * b);
            if(b > 0) {
                tminxz = 0;
                tmaxxz = t;
                if(t <= 0) return false;
            } else {
                tminxz = t;
                tmaxxz = 1;
                if(t >= 1) return false;
            }
        }
        p1y += halfH;
        let min;
        let normal=hit.normal.elements,position=hit.position.elements;
        if(tmaxxz <= tminy || tmaxy <= tminxz) return false;
        if(tminxz < tminy) {
            min = tminy;
            if(min === 0) return false;
            normal[0]=0;normal[1]=dy > 0 ? -1 : 1;normal[2]=0;
        } else {
            min = tminxz;
            if(min === 0) return false;
            normal[0]=beginX + dx * min;normal[1]=0;normal[2]=beginZ + dz * min;
            let invLen = Math.sqrt(normal[0] * normal[0] + normal[1] * normal[1] + normal[2] * normal[2]);
            if(invLen > 0) invLen = 1 / invLen;
            normal[0] *= invLen;normal[1] *= invLen;normal[2] *= invLen;
            let s = cosTheta;
            normal[0] *= s;normal[1] *= s;normal[2] *= s;
            normal[1] += this.sinTheta;
        }
        position[0]=beginX + min * dx;position[1]=p1y + min * dy;position[2]=beginZ + min * dz;
        hit.fraction = min;
        return true;
    }
}